% 阶跃响应测试
pos_pid = newPidControler(a_sj,tor_motor,[0.1;0.1],[1;1]);
a_sj.reset_state(initial_state);
pos_pid.pid_reset();
% pos_pid.pid_reset(kp,ki,kd);
%先把pid架设好
con_value =[0;0];
sample_time_count = sample_period;
for i = 1:simu_step
    if(sample_time_count >= sample_period)
        pos_pid.set_target([step_value 0 0 ]);
        debug_info(:,i,:) = pos_pid.get_con_value(a_sj,a_sj.joint_state,sample_period);
        con_value = debug_info(:,i,1);
        sample_time_count = sample_time_count-sample_period;
    else
        debug_info(:,i,:) = debug_info(:,i-1,:);

    end
    motor_res(:,i,:) = tor_motor.simu_loop(a_sj,simu_period,con_value);
    state_info(:,i,:) = (a_sj.simu_loop(motor_res(:,i,1),simu_period));
    sample_time_count = sample_time_count+simu_period;
end

%绘图
figure;
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,1),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
yline(step_value,'r--');
legend('n=10','n=50','target')
title("pos hold  theta");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,2),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,2),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
yline(0,'r--');
legend('n=10','n=50','target');
title("pos hold omega");